#ifndef LANDMARK_NAVIGATION_H
#define LANDMARK_NAVIGATION_H
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

#include <iostream>
#include <thread>
#include <map>
#include <yaml-cpp/yaml.h>
#include <Eigen/Eigen>
#include <math.h>
#include "tinyxml2.h"

#include "rclcpp/rclcpp.hpp"
#include <sensor_msgs/msg/image.hpp>
#include <agv_msgs/msg/odometry_lite.hpp>
#include <agv_msgs/msg/pose_with_confidence.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <eigen_conversions/eigen_msg.h>
#include <geometry_msgs/msg/pose.hpp>

#include "tf2_ros/transform_broadcaster.h"
#include <tf/transform_broadcaster.h>

#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"

using namespace Eigen;
using namespace tinyxml2;
using namespace std;


struct qrmapPoint
{
  float x;
  float y;
  float angle;
};

class Landmark_navigation
{
public:
  Landmark_navigation();
  ~Landmark_navigation();

  bool qrmap_Init();
  void HandLandmarkCallback(const geometry_msgs::msg::PoseStamped& odom_link_get);
  void sendTransform_task(geometry_msgs::msg::TransformStamped *odom_trans);
public:
  float Xpos, Ypos;
  float Apos;
  uint32_t TAG = 0;
  Matrix4d T_RtoQ, T_QtoM, T_RtoM, con_mat, T_BtoM;


  geometry_msgs::msg::Pose odom_link_get_;
  Eigen::Affine3d dm_to_base;
  Eigen::Matrix4d dm_to_base_;
  Matrix4d T_RtoO, T_OtoM;

  int agv_navigation_type;

  map<uint32_t, qrmapPoint> qrmap;

  rclcpp::Node node_handle;
  ros::Subscriber odom_link_sub_;
  ros::Publisher PublisherTrackedPose_;

  tf2_ros::TransformBroadcaster tf_broadcaster_;
  tf2_ros::TransformListener *listener;

  tf2::StampedTransform basetoodom;
  tf2::StampedTransform basetomap;
  tf2::StampedTransform odom_tf;//odom的tf

  tf2_ros::Buffer tfbuffer_;

  geometry_msgs::msg::TransformStamped AGVtf;
  geometry_msgs::msg::TransformStamped base_to_map;
  geometry_msgs::msg::TransformStamped odom_trans;
  agv_msgs::msg::PoseWithConfidence cur_pose_msg_;

  string landmark_xml_path;
};

#endif // LANDMARK_NAVIGATION_H
